#include <iostream>
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <chrono>
#include <atomic>
#include <thread>
#include <unitree/common/thread/thread.hpp>
#include <unitree/common/time/time_tool.hpp>
#include <unitree/idl/go2/LowCmd_.hpp>
#include <unitree/idl/go2/LowState_.hpp>
#include <unitree/robot/channel/channel_publisher.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>

using namespace unitree::common;
using namespace unitree::robot;
using namespace std;

#define TOPIC_LOWSTATE "rt/lowstate"

class Custom {
public:
    explicit Custom()
    {
    }

    ~Custom()
    {
    }

    void Init();
    int callbackCount;

private:
    void InitLowCmd();
    void LowStateMessageHandler(const void* messages);

private:
    unitree_go::msg::dds_::LowState_ low_state {};

    /*subscriber*/
    ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber;

};

void Custom::Init()
{
    /*create subscriber*/
    lowstate_subscriber.reset(new ChannelSubscriber<unitree_go::msg::dds_::LowState_>(TOPIC_LOWSTATE));
    lowstate_subscriber->InitChannel(std::bind(&Custom::LowStateMessageHandler, this, std::placeholders::_1), 1);

    callbackCount = 0;
}

void Custom::LowStateMessageHandler(const void* message)
{
    low_state = *(unitree_go::msg::dds_::LowState_*)message;
    auto bms_data = low_state.bms_state();

    // 打印 BMS 信息
    cout << "Battery:" << endl;
    cout << "    status   : " << (int)bms_data.status() << endl;
    cout << "    soc      : " << (int)bms_data.soc() << endl;
    cout << "    current  : " << (int)bms_data.current() << endl;
    cout << "    cycle    : " << (int)bms_data.cycle() << endl;
    cout << "    bq_ntc0  : " << (int)bms_data.bq_ntc()[0] << endl;
    cout << "    bq_ntc1  : " << (int)bms_data.bq_ntc()[1] << endl;
    cout << "    mcu_ntc0 : " << (int)bms_data.mcu_ntc()[0] << endl;
    cout << "    mcu_ntc1 : " << (int)bms_data.mcu_ntc()[1] << endl;
    cout << "    cell_vol : "
        << (int)bms_data.cell_vol()[0] << " "
        << (int)bms_data.cell_vol()[1] << " "
        << (int)bms_data.cell_vol()[2] << " "
        << (int)bms_data.cell_vol()[3] << " "
        << (int)bms_data.cell_vol()[4] << " "
        << (int)bms_data.cell_vol()[5] << " "
        << (int)bms_data.cell_vol()[6] << " "
        << (int)bms_data.cell_vol()[7] << " "
        << (int)bms_data.cell_vol()[8] << " "
        << (int)bms_data.cell_vol()[9] << " "
        << (int)bms_data.cell_vol()[10] << " "
        << (int)bms_data.cell_vol()[11] << " "
        << (int)bms_data.cell_vol()[12] << " "
        << (int)bms_data.cell_vol()[13] << " "
        << (int)bms_data.cell_vol()[14] << " "
        << (int)bms_data.cell_vol()[15] << endl;
    cout << "" << endl;

    callbackCount++;
}

int main(int argc, const char** argv)
{
    ChannelFactory::Instance()->Init(0, "eth0");

    cout << "Factory Init" << endl;

    Custom custom;
    custom.Init();

    cout << "custom Init" << endl;

    int prevCount = 0;
    auto startTime = std::chrono::steady_clock::now();

    while (1) {
        this_thread::sleep_for(std::chrono::seconds(1));  // 每秒计算一次频率
        int currentCount = custom.callbackCount;
        int frequency = currentCount - prevCount;
        prevCount = currentCount;
        auto endTime = std::chrono::steady_clock::now();
        auto elapsedTime = std::chrono::duration_cast<std::chrono::seconds>(endTime - startTime).count();

        cout << "Callback frequency: " << frequency << " Hz" << endl;
        startTime = endTime;
    }

    return 0;
}

